/**
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  * @file       chassis_behaviour.c/h
  * @brief      2025赛季哨兵，根据遥控器的值，决定底盘行为
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. 完成
  *  V1.1.0     Nov-11-2019     RM              1. 添加一些注释
  *  V2.0.0     Jan-1-2024      罗威昊           1. 添加小陀螺模式
  *  V3.0.0     Dec-20-2024     冉文治           1. 添加普通导航模式和变速小陀螺导航模式
  *
  @verbatim
  ==============================================================================
    如果要添加一个新的行为模式
    1.首先，在chassis_behaviour.h文件中， 添加一个新行为名字在 chassis_behaviour_e
    erum
    {
        ...
        ...
        CHASSIS_XXX_XXX, // 新添加的
    }chassis_behaviour_e,
    2. 实现一个新的函数 chassis_xxx_xxx_control(fp32 *vx, fp32 *vy, fp32 *wz, chassis_move_t * chassis )
        "vx,vy,wz" 参数是底盘运动控制输入量
        第一个参数: 'vx' 通常控制纵向移动,正值 前进， 负值 后退
        第二个参数: 'vy' 通常控制横向移动,正值 左移, 负值 右移
        第三个参数: 'wz' 可能是角度控制或者旋转速度控制
        在这个新的函数, 你能给 "vx","vy",and "wz" 赋值想要的速度参数
    3.  在"chassis_behaviour_mode_set"这个函数中，添加新的逻辑判断，给chassis_behaviour_mode赋值成CHASSIS_XXX_XXX
        在函数最后，添加"else if(chassis_behaviour_mode == CHASSIS_XXX_XXX)" ,然后选择一种底盘控制模式
    4.  在"chassis_behaviour_control_set" 函数的最后，添加
        else if(chassis_behaviour_mode == CHASSIS_XXX_XXX)
        {
            chassis_xxx_xxx_control(vx_set, vy_set, angle_set, chassis_move_rc_to_vector);
        }
  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  */

#include "chassis_behaviour.h"
#include "chassis_task.h"
#include "arm_math.h"
#include "gimbal_behaviour.h"
#include "usb_task.h"
#include "referee.h"
#include "referee_usart_task.h"

/**
  * @brief          底盘无力的行为状态机下，底盘模式是raw，故而设定值会直接发送到can总线上故而将设定值都设置为0
  * @author         RM
  * @param          vx_can_set 前进的速度 设定值将直接发送到can总线上
  * @param          vy_can_set 左右的速度 设定值将直接发送到can总线上
  * @param          wz_can_set 旋转的速度 设定值将直接发送到can总线上
  * @param          chassis_move_rc_to_vector 底盘数据
  * @retval         返回空
  */
static void chassis_zero_force_control(fp32 *vx_can_set, fp32 *vy_can_set, fp32 *wz_can_set, chassis_move_t *chassis_move_rc_to_vector);

/**
  * @brief          底盘不移动的行为状态机下，底盘模式是不跟随角度，
  * @author         RM
  * @param          vx_set 前进的速度,正值 前进速度,负值 后退速度
  * @param          vy_set 左右的速度,正值 左移速度,负值 右移速度
  * @param          wz_set 旋转的速度，旋转速度是控制底盘的底盘角速度
  * @param          chassis_move_rc_to_vector 底盘数据
  * @retval         返回空
  */
static void chassis_no_move_control(fp32 *vx_set, fp32 *vy_set, fp32 *wz_set, chassis_move_t *chassis_move_rc_to_vector);

/**
  * @brief          底盘跟随云台的行为状态机下，底盘模式是跟随云台角度，底盘旋转速度会根据角度差计算底盘旋转的角速度
  * @author         RM
  * @param          vx_set 前进的速度,正值 前进速度,负值 后退速度
  * @param          vy_set 左右的速度,正值 左移速度,负值 右移速度
  * @param          angle_set 底盘与云台控制到的相对角度
  * @param          chassis_move_rc_to_vector 底盘数据
  * @retval         返回空
  */
static void chassis_infantry_follow_gimbal_yaw_control(fp32 *vx_set, fp32 *vy_set, fp32 *angle_set, chassis_move_t *chassis_move_rc_to_vector);

/**
  * @brief          底盘小陀螺遥控模式
  * @author         冉文治
  * @param          vx_set 前进的速度,正值 前进速度,负值 后退速度
  * @param          vy_set 左右的速度,正值 左移速度,负值 右移速度
  * @param          wz_set 底盘角速度设定值
  * @param          chassis_move_rc_to_vector 底盘数据
  * @retval         返回空
  */
static void chassis_infantry_rotation_control(fp32 *vx_set, fp32 *vy_set, fp32 *wz_set, chassis_move_t *chassis_move_auto_to_vector);

/**
  * @brief          底盘正常导航模式
  * @author         冉文治
  * @param          vx_set 前进的速度,正值 前进速度,负值 后退速度
  * @param          vy_set 左右的速度,正值 左移速度,负值 右移速度
  * @param          angle_set 底盘与云台控制到的相对角度
  * @param          chassis_move_rc_to_vector 底盘数据
  * @retval         返回空
  */
static void chassis_sentinel_follow_radar_yaw_control(fp32 *vx_set, fp32 *vy_set, fp32 *wz_set, chassis_move_t *chassis_move_rc_to_vector);

/**
  * @brief          底盘小陀螺导航模式
  * @author         冉文治
  * @param          vx_set 前进的速度,正值 前进速度,负值 后退速度
  * @param          vy_set 左右的速度,正值 左移速度,负值 右移速度
  * @param          wz_set 底盘角速度设定值
  * @param          chassis_move_rc_to_vector 底盘数据
  * @retval         返回空
  */
static void chassis_sentinel_rotation_control(fp32 *vx_set, fp32 *vy_set, fp32 *wz_set, chassis_move_t *chassis_move_rc_to_vector);

// 底盘行为模式变量
chassis_behaviour_e chassis_behaviour_mode = CHASSIS_ZERO_FORCE;

/**
  * @brief          通过逻辑判断，赋值"chassis_behaviour_mode"成哪种模式
  * @param          chassis_move_mode: 底盘数据
  * @retval         none
  */
void chassis_behaviour_mode_set(chassis_move_t *chassis_move_mode)
{
    if (chassis_move_mode == NULL)
    {
        return;
    }

    static fp32 robot_hurt_time = 0.0f;

    // 遥控器设置模式
    // 右侧摇杆在中间
    if (switch_is_mid(chassis_move_mode->chassis_RC->rc.s[CHASSIS_MODE_CHANNEL]))
    {
        if (chassis_move_mode->chassis_RC->rc.ch[4] == 660)
        {
            chassis_behaviour_mode = CHASSIS_ROTATION_INFANTRY_FOLLOW_GIMBAL_YAW;   // 步兵小陀螺
        }
        else
        {
            chassis_behaviour_mode = CHASSIS_INFANTRY_FOLLOW_GIMBAL_YAW;            // 步兵手操
        }
    }
    // 右侧遥感在上面
    else if (switch_is_up(chassis_move_mode->chassis_RC->rc.s[CHASSIS_MODE_CHANNEL]))
    {
        if (game_state_t.game_progress == 4)
        {
            chassis_behaviour_mode = CHASSIS_SENTINEL_FOLLOW_RADAR_YAW;             // 导航
            if ((chassis_move_mode->robot_status->current_HP != chassis_move_mode->last_robot_HP && chassis_move_mode->robot_hurt->HP_deduction_reason == 0) || robot_hurt_time != 0.0f)
            {
                chassis_behaviour_mode = CHASSIS_ROTATION_SENTINEL_FOLLOW_GIMBAL_YAW;
                robot_hurt_time += chassis_move_mode->dt;

                if (chassis_move_mode->robot_status->current_HP != chassis_move_mode->last_robot_HP && chassis_move_mode->robot_hurt->HP_deduction_reason == 0)
                {
                    robot_hurt_time = 0.1f;
                }

                if (robot_hurt_time >= 10.0f && chassis_move_mode->robot_status->current_HP == chassis_move_mode->last_robot_HP)
                {
                    chassis_behaviour_mode = CHASSIS_SENTINEL_FOLLOW_RADAR_YAW;             // 导航
                    robot_hurt_time = 0.0f;
                }
            }
        }
        else
        {
            if (chassis_move_mode->chassis_RC->rc.ch[4] == 660)
            {
                chassis_behaviour_mode = CHASSIS_ROTATION_SENTINEL_FOLLOW_GIMBAL_YAW;   // 小陀螺旋转导航
            }
            else
            {
                chassis_behaviour_mode = CHASSIS_SENTINEL_FOLLOW_RADAR_YAW;             // 导航
            }
        }
    }
    // 右侧遥感在下面
    else if (switch_is_down(chassis_move_mode->chassis_RC->rc.s[CHASSIS_MODE_CHANNEL]))
    {
        chassis_behaviour_mode = CHASSIS_NO_MOVE;               // 挂机
    }

    // 初始化模式下，底盘不动
    if (gimbal_cmd_to_chassis_stop())
    {
        chassis_behaviour_mode = CHASSIS_NO_MOVE;   // 挂机模式
    }

    // 根据行为选择底盘模式
    if (chassis_behaviour_mode == CHASSIS_ZERO_FORCE)   // 底盘无力，即初始化
    {
        chassis_move_mode->chassis_mode = CHASSIS_VECTOR_RAW;   // 底盘初始化
    }
    else if (chassis_behaviour_mode == CHASSIS_NO_MOVE)     // 挂机模式
    {
        chassis_move_mode->chassis_mode = CHASSIS_VECTOR_NO_FOLLOW_YAW;     // 底盘挂机
    }
	else if (chassis_behaviour_mode == CHASSIS_INFANTRY_FOLLOW_GIMBAL_YAW)      // 遥控模式
    {
        chassis_move_mode->chassis_mode = CHASSIS_VECTOR_FOLLOW_GIMBAL_YAW;     // 底盘遥控，即底盘跟随云台
    }
	else if (chassis_behaviour_mode == CHASSIS_ROTATION_INFANTRY_FOLLOW_GIMBAL_YAW)
	{
		chassis_move_mode->chassis_mode = CHASSIS_VECTOR_ROTATION;		// 底盘小陀螺
	}
    else if (chassis_behaviour_mode == CHASSIS_ROTATION_SENTINEL_FOLLOW_GIMBAL_YAW)   // 小陀螺导航模式
    {
        chassis_move_mode->chassis_mode = CHASSIS_AUTO_ROTATION;    // 底盘小陀螺导航
    }
    else if(chassis_behaviour_mode == CHASSIS_SENTINEL_FOLLOW_RADAR_YAW)      // 正常导航模式
    {
        chassis_move_mode->chassis_mode = CHASSIS_AUTO_FOLLOW_RADAR_YAW;      // 底盘小陀螺导航
    }
}

/**
  * @brief          设置控制量.根据不同底盘控制模式，三个参数会控制不同运动.在这个函数里面，会调用不同的控制函数.
  * @param          vx_set 通常控制纵向移动.
  * @param          vy_set 通常控制横向移动.
  * @param          angle_set 通常控制旋转运动.
  * @param          chassis_move_rc_to_vector  包括底盘所有信息.
  * @retval         none
  */
void chassis_behaviour_control_set(fp32 *vx_set, fp32 *vy_set, fp32 *angle_set, chassis_move_t *chassis_move_rc_to_vector)
{

    if (vx_set == NULL || vy_set == NULL || angle_set == NULL || chassis_move_rc_to_vector == NULL)
    {
        return;
    }

    if (chassis_behaviour_mode == CHASSIS_ZERO_FORCE)                       // 底盘无力，即初始化
    {
        chassis_zero_force_control(vx_set, vy_set, angle_set, chassis_move_rc_to_vector);
    }
    else if (chassis_behaviour_mode == CHASSIS_NO_MOVE)                     // 挂机
    {
        chassis_no_move_control(vx_set, vy_set, angle_set, chassis_move_rc_to_vector);
    }
	else if (chassis_behaviour_mode == CHASSIS_INFANTRY_FOLLOW_GIMBAL_YAW)  // 底盘跟随云台
    {
        chassis_infantry_follow_gimbal_yaw_control(vx_set, vy_set, angle_set, chassis_move_rc_to_vector);
    }
	else if (chassis_behaviour_mode == CHASSIS_ROTATION_INFANTRY_FOLLOW_GIMBAL_YAW)		// 底盘遥控小陀螺
	{
			chassis_infantry_rotation_control(vx_set, vy_set, angle_set, chassis_move_rc_to_vector);
	}
    else if (chassis_behaviour_mode == CHASSIS_ROTATION_SENTINEL_FOLLOW_GIMBAL_YAW)           // 底盘小陀螺导航
    {
        chassis_sentinel_rotation_control(vx_set, vy_set, angle_set, chassis_move_rc_to_vector);
    }
    else if(chassis_behaviour_mode == CHASSIS_SENTINEL_FOLLOW_RADAR_YAW)              // 底盘正常导航
    {
        chassis_sentinel_follow_radar_yaw_control(vx_set, vy_set, angle_set, chassis_move_rc_to_vector);
    }
}

/**
  * @brief          底盘无力的行为状态机下，底盘模式是raw，故而设定值会直接发送到can总线上故而将设定值都设置为0
  * @author         RM
  * @param          vx_can_set 前进的速度 设定值将直接发送到can总线上
  * @param          vy_can_set 左右的速度 设定值将直接发送到can总线上
  * @param          wz_can_set 旋转的速度 设定值将直接发送到can总线上
  * @param          chassis_move_rc_to_vector 底盘数据
  * @retval         返回空
  */
static void chassis_zero_force_control(fp32 *vx_can_set, fp32 *vy_can_set, fp32 *wz_can_set, chassis_move_t *chassis_move_rc_to_vector)
{
    if (vx_can_set == NULL || vy_can_set == NULL || wz_can_set == NULL || chassis_move_rc_to_vector == NULL)
    {
        return;
    }

    *vx_can_set = 0.0f;
    *vy_can_set = 0.0f;
    *wz_can_set = 0.0f;
}

/**
  * @brief          底盘不移动的行为状态机下，底盘模式是不跟随角度，
  * @author         RM
  * @param          vx_set 前进的速度,正值 前进速度,负值 后退速度
  * @param          vy_set 左右的速度,正值 左移速度,负值 右移速度
  * @param          wz_set 旋转的速度，旋转速度是控制底盘的底盘角速度
  * @param          chassis_move_rc_to_vector 底盘数据
  * @retval         返回空
  */
static void chassis_no_move_control(fp32 *vx_set, fp32 *vy_set, fp32 *wz_set, chassis_move_t *chassis_move_rc_to_vector)
{
    if (vx_set == NULL || vy_set == NULL || wz_set == NULL || chassis_move_rc_to_vector == NULL)
    {
        return;
    }

    *vx_set = 0.0f;
    *vy_set = 0.0f;
    *wz_set = 0.0f;
}

/**
  * @brief          底盘跟随云台的行为状态机下，底盘模式是跟随云台角度，底盘旋转速度会根据角度差计算底盘旋转的角速度
  * @author         RM
  * @param          vx_set 前进的速度,正值 前进速度,负值 后退速度
  * @param          vy_set 左右的速度,正值 左移速度,负值 右移速度
  * @param          angle_set 底盘与云台控制到的相对角度
  * @param          chassis_move_rc_to_vector 底盘数据
  * @retval         返回空
  */
static void chassis_infantry_follow_gimbal_yaw_control(fp32 *vx_set, fp32 *vy_set, fp32 *angle_set, chassis_move_t *chassis_move_rc_to_vector)
{
    if (vx_set == NULL || vy_set == NULL || angle_set == NULL || chassis_move_rc_to_vector == NULL)
    {
        return;
    }

    chassis_rc_to_control_vector(vx_set, vy_set, chassis_move_rc_to_vector);
    *angle_set = 0.0f;
}

/**
  * @brief          底盘小陀螺遥控模式
  * @author         冉文治
  * @param          vx_set 前进的速度,正值 前进速度,负值 后退速度
  * @param          vy_set 左右的速度,正值 左移速度,负值 右移速度
  * @param          wz_set 底盘角速度设定值
  * @param          chassis_move_rc_to_vector 底盘数据
  * @retval         返回空
  */
static void chassis_infantry_rotation_control(fp32 *vx_set, fp32 *vy_set, fp32 *wz_set, chassis_move_t *chassis_move_rc_to_vector)
{
		if (vx_set == NULL || vy_set == NULL || wz_set == NULL || chassis_move_rc_to_vector == NULL)
    {
        return;
    }

	chassis_rc_to_control_vector(vx_set, vy_set, chassis_move_rc_to_vector);
    *wz_set = CHASSIS_SPIN_MAIN_SPEED - (fabs(chassis_move_rc_to_vector->vx_set + fabs(chassis_move_rc_to_vector->vy_set)) * CHASSIS_SPIN_LOW_SEN);
}

/**
  * @brief          底盘正常导航模式
  * @author         冉文治
  * @param          vx_set 前进的速度,正值 前进速度,负值 后退速度
  * @param          vy_set 左右的速度,正值 左移速度,负值 右移速度
  * @param          angle_set 底盘与云台控制到的相对角度
  * @param          chassis_move_auto_to_vector 底盘数据
  * @retval         返回空
  */
static void chassis_sentinel_follow_radar_yaw_control(fp32 *vx_set, fp32 *vy_set, fp32 *angle_set, chassis_move_t *chassis_move_auto_to_vector)
{
    if (vx_set == NULL || vy_set == NULL || angle_set == NULL || chassis_move_auto_to_vector == NULL)
    {
        return;
    }

	*vx_set = chassis_move_auto_to_vector->ld_ctrl->Linear_Twist_X;
	*vy_set = chassis_move_auto_to_vector->ld_ctrl->Linear_Twist_Y;
    *angle_set = -atan2(*vy_set, *vx_set);
}

/**
  * @brief          底盘小陀螺导航模式
  * @author         冉文治
  * @param          vx_set 前进的速度,正值 前进速度,负值 后退速度
  * @param          vy_set 左右的速度,正值 左移速度,负值 右移速度
  * @param          wz_set 底盘角速度设定值
  * @param          chassis_move_rc_to_vector 底盘数据
  * @retval         返回空
  */
static void chassis_sentinel_rotation_control(fp32 *vx_set, fp32 *vy_set, fp32 *wz_set, chassis_move_t *chassis_move_auto_to_vector)
{
    if (vx_set == NULL || vy_set == NULL || wz_set == NULL || chassis_move_auto_to_vector == NULL)
    {
        return;
    }

    *vx_set = chassis_move_auto_to_vector->ld_ctrl->Linear_Twist_X;
    *vy_set = chassis_move_auto_to_vector->ld_ctrl->Linear_Twist_Y;
    *wz_set = CHASSIS_SPIN_MAIN_SPEED - (fabs(chassis_move_auto_to_vector->vx_set + fabs(chassis_move_auto_to_vector->vy_set)) * CHASSIS_SPIN_LOW_SEN);
}
